
for i = 1:n
    qi = Q(i, :);
    x_real(3*i-2:3*i, 1) = robot_real.fkine(qi).t;
end

alpha = 0.01;
beta = 0.9;

iter_max = 20;
delta_para = zeros(1, 24);

finish = false;
delta(1) = norm(calculate_error(delta_para, DH_Para, x_real, Q));
for k = 1:iter_max
    
    for i = 1:n
        qi = Q(i, :);
        M(3*i-2:3*i, :) = calculate_Jacobian(delta_para, DH_Para, qi);
    end

    delta_x = calculate_error(delta_para, DH_Para, x_real, Q);
    delta_l = pinv(M) * delta_x;
    
    tk = 1;
    while norm(calculate_error(delta_para + tk * delta_l', DH_Para, x_real, Q)) > norm(delta_x)
        tk = beta * tk;
        if tk < 0.1
            finish = true;
            delta_l = 0 * delta_l;
            break
        end
    end
    
    delta_para = delta_para + tk * delta_l';
    delta(k + 1) = norm(calculate_error(delta_para, DH_Para, x_real, Q));
    
    if finish
        break;
    end
    
end

delta_para(23) = atan(-delta_para(12) / (delta_para(18) + d6)) + delta_para(23);
delta_para(18) = sqrt(delta_para(12)^2 + (delta_para(18) + d6)^2) - d6;
delta_para(12) = 0;
delta_para(17) = delta_para(17) + (delta_para(18) + d6) * sin(delta_para(6));
delta_para(6) = 0;

disp("alpha error: ");
disp(delta_para(1:6));
disp("a error: ");
disp(delta_para(7:12));
disp("d error: ");
disp(delta_para(13:18));
disp("theta error: ");
disp(delta_para(19:24));

figure(1);
plot(delta);